#ifndef __PLANNER_BIND_ROBOT_HPP__
#define __PLANNER_BIND_ROBOT_HPP__

#include "Planner.hpp"
#include <vector>
#include <queue>
#include <algorithm>

/**
 * @brief 规划器-绑定机器人策略
 * @note 为每个泊位分配固定的机器人，机器人仅在辖区内活动
 */
class PlannerBindRobot : public Planner
{
private:
    int frameID = -1; // 当前帧
    const Map *map = nullptr; // 地图
    std::vector<Berth> *berths = nullptr; // 泊位
    const std::vector<Goods *> *goods_list = nullptr; // 货物
    const std::vector<BoatDispatcher> *boatDispatchers = nullptr; // 机器人调度器
    const std::vector<RobotDispatcher> *robotDispatchers = nullptr; // 船调度器
    std::vector<int> bindBerth = std::vector<int>(ROBOT_NUM, INVALID_BERTH_ID); // 机器人绑定的泊位
    std::vector<std::priority_queue<PlanForLand>> berthPlans = 
        std::vector<std::priority_queue<PlanForLand>>(BERTH_NUM, std::priority_queue<PlanForLand>()); // 各泊位的优先队列
    std::vector<bool> berthOccupied = std::vector<bool>(BERTH_NUM, false); // 泊位被占用

public:
    /**
     * @brief 绑定机器人和泊位
     * @note 按面积对可用泊位排序，将机器人均匀分配，多余的机器人分给面积大的泊位
     */
    void bindRobotAndBerth() {
        // 按面积对可用泊位排序
        struct berthArea {
            int id, S;
            berthArea(int id, int S) : id(id), S(S) {}
        };
        std::vector<berthArea> berthAreas;
        for (auto const &berth: *berths) {
            if (berth.isLocked()) {
                continue;
            }
            berthAreas.push_back(berthArea(berth.id, map->getBerthAreaS(berth.id)));
        }
        std::sort(berthAreas.begin(), berthAreas.end(), [](berthArea x1, berthArea x2) { return x1.S > x2.S; });

        // 将机器人数量均匀分配
        std::vector<int> robotNum(BERTH_NUM, 0); // 泊位应分得的机器人数量
        int loopNum = 0;
        while (loopNum < ROBOT_NUM) {
            for (auto const &berth: berthAreas) {
                ++robotNum[berth.id];
                if (++loopNum > ROBOT_NUM) {
                    break;
                }
            }
        }

        // 按距离绑定机器人与泊位
        struct plan {
            int r, b, l; // 机器人id，泊位id，长度
            plan(int r, int b, int l) : r(r), b(b), l(l) {}
            bool operator<(const plan &p) const { return l > p.l; }
        };
        std::priority_queue<plan> plans;
        for (int r = 0; r < ROBOT_NUM; ++r) {
            for (int b = 0; b < BERTH_NUM; ++b) {
                plans.push(plan(r, b, map->getBerthDistance(b, map->robotsInit[r])));
            }
        }
        while (!plans.empty()) {
            auto top = plans.top();
            plans.pop();
            if (robotNum[top.b] == 0) {
                continue;
            }
            if (bindBerth[top.r] == INVALID_BERTH_ID) {
                bindBerth[top.r] = top.b;
                --robotNum[top.b];
            }
        }
    }
    /**
     * @brief 初始化
     * @param map 地图
     * @param berths 泊位列表
     * @param goods_list 货物列表
     * @param boatDispatchers 船调度器列表
     * @param robotDispatchers 机器人调度器列表
     * @note 初始化阶段调用一次，连接到可用的地图和泊位、货物列表等，由外部保证这些数据实时刷新
     */
    void init(const Map *map, std::vector<Berth> *berths, const std::vector<Goods *> *goods_list,
        const std::vector<BoatDispatcher> *boatDispatchers,
        const std::vector<RobotDispatcher> *robotDispatchers) override {

        this->map = map;
        this->berths = berths;
        this->goods_list = goods_list;
        this->boatDispatchers = boatDispatchers;
        this->robotDispatchers = robotDispatchers;

        // 为机器人绑定泊位
        bindRobotAndBerth();
    }
    /**
     * @brief 刷新一帧
     * @param frameID 当前帧
     * @param newGoodsNum 新增货物数
     * @note  每帧运行一次（可能存在跳帧），新增货物数用于倒序遍历goods_list
     */
    void refresh(int frameID, int newGoodsNum) override {
        this->frameID = frameID;
        int remainTime = TIME_FRAME_MAX - frameID; // 剩余时间

        // 刷新泊位占用状态
        std::fill(berthOccupied.begin(), berthOccupied.end(), false);
        for (auto const &boatDispatcher : *boatDispatchers) {
            int berthID = boatDispatcher.boat->berthID;
            if (berthID == INVALID_BERTH_ID || berthID < 0 || berthID >= BERTH_NUM) {
                continue;
            }
            berthOccupied[berthID] = true;
        }

        // 维护优先队列
        auto goods = goods_list->rbegin();
        for (int i = 0; i < newGoodsNum && goods != goods_list->rend(); ++i, ++goods) {
            if (!(*goods)->isAvailable(frameID)) {
                continue;
            }
            int berth = map->getNearestBerth(**goods); // 泊位号
            int dis = map->getBerthDistance(berth, **goods); // 泊位距离
            int time = dis + PARAMS.ROBOT_MOVE_MARGIN_TIME; // 取货需要的时间
            if (time > remainTime || !(*goods)->isAvailable(frameID + time)) { // 来不及取了
                continue;
            }
            float value = (*goods)->value / static_cast<float>(time);
            berthPlans[berth].push(PlanForLand(*goods, berth, value));
        }
    }
    /**
     * @brief 给指定机器人分配任务
     * @param robot 指定机器人
     * @return PlanForLand 分配的任务
     */
    PlanForLand allocatePlan(const Robot *robot) override {
        PlanForLand ret;
        int berth = bindBerth[robot->id]; // 获取机器人绑定的泊位
        while (!berthPlans[berth].empty()) {
            auto goods = berthPlans[berth].top().goods;
            // 机器人空闲时总是在泊位，因此以货物到泊位的距离作为机器人到货物的距离估值
            int dis = map->getBerthDistance(berth, *goods) + PARAMS.ROBOT_MOVE_MARGIN_TIME;
            if (goods->isAvailable(frameID + dis)) {
                ret = berthPlans[berth].top();
                berthPlans[berth].pop();
                if (map->getNearestBerth(*robot) == berth) { // 机器人进入泊位辖区后就不再退出
                    ret.setBerthArea(berth);
                }
                break;
            } else {
                berthPlans[berth].pop();
            }
        }
        return ret;
    }
    /**
     * @brief 给指定船分配任务
     * @param boat 指定船
     * @return PlanForSea 分配的任务
     */
    PlanForSea allocatePlan(const Boat *boat) override {
        float bestValue = 0;
        int bestBerth = INVALID_BERTH_ID;
        int timeOffset = 0;
        for (auto berth : *berths) {
            if (berth.isLocked() || berthOccupied[berth.id]) {
                continue;
            }
            int num = std::min<int>(boat->volume, berth.getGoodsNum());
            int time = berth.distance + num / berth.velocity;
            float value = static_cast<float>(berth.getValue(num)) / time;
            if (value > bestValue) {
                bestValue = value;
                bestBerth = berth.id;
                timeOffset = time;
            }
        }
        if (bestValue != 0) {
            int leaveTime = frameID + timeOffset;
            int minLeaveTime = TIME_FRAME_MAX - (*berths)[boat->id].distance - 1; // 超过这个时间就来不及送了
            return PlanForSea(bestBerth, std::min<int>(leaveTime, minLeaveTime));
        } else {
            return PlanForSea();
        }
    }
};

#endif // __PLANNER_BIND_ROBOT_HPP__